%FIRST-ORDER POLYNOMIAL KALMAN FILTER AND SINUSOIDAL MEASUREMENT
clear all
close all
ORDER=2;
PHIS=0.;
TS=.1;
XH=0.;
XDH=0.;
SIGNOISE=1.;
PHI=[1 TS ;
0 1];
P=[99999999 0;
0 999999999];
IDNP=eye(ORDER);
Q=[PHIS*TS^3/3 PHIS*TS*TS/2;
PHIS*TS*TS/2 PHIS*TS];
RMAT=SIGNOISE^2;
HMAT=[1 0];
count=0;
for T=0:TS:20
    M=PHI*P*PHI'+Q;
    K = M*HMAT'/(HMAT*M*HMAT' + RMAT);
    P = (IDNP - K*HMAT)*M;
    XNOISE=SIGNOISE*randn;
    X=sin(T);
    XD=cos(T);
    XS=X+XNOISE;
    RES=XS-XH-TS*XDH;
    XH=XH+XDH*TS+K(1,1)*RES;
    XDH=XDH+K(2,1)*RES;
    count=count+1;
    ArrayT(count)=T;
    ArrayX(count)=X;
    ArrayXS(count)=XS;
    ArrayXH(count)=XH;
    ArrayXD(count)=XD;
    ArrayXDH(count)=XDH;
end
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXS),grid
xlabel('Time (Sec)')
ylabel('Measurement and True Signal')
axis([0 20 -3 3])
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('Time (Sec)')
ylabel('Estimate and True Signal')
axis([0 20 -2 2])
figure
plot(ArrayT,ArrayXD,ArrayT,ArrayXDH),grid
xlabel('Time (Sec)')
ylabel('XD and XDH')
axis([0 20 -4 4])